/**
* @file         algori_drtuning.cpp
* @author       LDRobot (support@ldrobot.com)
* @brief         
* @version      0.1
* @date         2022.07.27
* @note          
* @copyright    Copyright (c) 2022  SHENZHEN LDROBOT CO., LTD. All rights reserved.
**/

#include "algori_drtuning.h"

AlgoriDrtuning::AlgoriDrtuning() {

}

AlgoriDrtuning::~AlgoriDrtuning() {
  
}


// 灰度重心法1
cv::Point AlgoriDrtuning::getGrayCenter(cv::Mat& img_gray) {
  const int T = 20; //根据实际情况设定固定阈值
	cv::Point center; //中心点
	int i, j;
	double sumval = 0; 
	cv::MatIterator_<uint8_t> it, end;
	//获取图像各点灰度值总和
	for (it = img_gray.begin<uint8_t>(), end = img_gray.end<uint8_t>(); it != end; it++) {
		((*it) > T) ? sumval += (*it) : 0; //小于阈值，取0
	}
	center.x = center.y = 0;
	double x = 0, y = 0;
	for (int i = 0; i < img_gray.cols; i++) {
		for (int j = 0; j < img_gray.rows; j++) {
			double s = img_gray.at<uint8_t>(j, i); //取当前点灰度值
			if (s < T) 
				s = 0; 
			x += i * s / sumval;
			y += j * s / sumval;
		}
	}
	center.x = cvRound(x);
	center.y = cvRound(y);
#if (SHOW_CIR)
	std::cout << "[getGrayCenter] rows=" << img_gray.rows << "  cols=" << img_gray.cols << std::endl; 
	std::cout << "[getGrayCenter] x=" << x << "  y=" << y << std::endl;
#endif
	return center;
}


void AlgoriDrtuning::centerCircleAnalysis(cv::Mat& src_gray_image, cv::Vec3f& out_cir) {
  cv::Mat img_gry, img_cny;
  src_gray_image.copyTo(img_gry);
  cv::Mat img_gry_mid_filter;
  cv::Mat img_gry_threshold_filter;
  // 二值化阈值处理
  cv::threshold(img_gry, img_gry_threshold_filter, 100, 255, cv::THRESH_BINARY); // 突出亮点位置
#if (SHOW_CIR)
  cv::namedWindow("C threshold filter Picture", cv::WINDOW_FREERATIO);
  cv::imshow("C threshold filter Picture", img_gry_threshold_filter);
#endif
  // 中值滤波
  cv::medianBlur(img_gry_threshold_filter, img_gry_mid_filter, 11);
#if (SHOW_CIR)
  cv::namedWindow("C mid filter Picture", cv::WINDOW_FREERATIO);
  cv::imshow("C mid filter Picture", img_gry_mid_filter);
#endif
  // 高斯滤波
  cv::Mat img_gaussian_filter;
  cv::GaussianBlur(img_gry_mid_filter, img_gaussian_filter, cv::Size(3, 3), 3, 3);
  cv::GaussianBlur(img_gaussian_filter, img_gaussian_filter, cv::Size(3, 3), 3, 3);
#if (SHOW_CIR)
  cv::namedWindow("C Gaussian filter Picture", cv::WINDOW_FREERATIO);
  cv::imshow("C Gaussian filter Picture", img_gaussian_filter);
#endif

  // 求解质心位置
  cv::Point circle_center = getGrayCenter(img_gaussian_filter);
#if (SHOW_CIR)
  cv::Mat img_centroid;
  img_gaussian_filter.copyTo(img_centroid);
  cv::circle(img_centroid, circle_center, 10, cv::Scalar(0,0,0));
  cv::namedWindow("C Centroid Picture", cv::WINDOW_FREERATIO);
  cv::imshow("C Centroid Picture", img_centroid);
#endif

  out_cir.val[0] = circle_center.x;
  out_cir.val[1] = circle_center.y;
  out_cir.val[2] = 10;
}

size_t AlgoriDrtuning::edgeLinesAnalysis(cv::Mat& src_gray_image, 
                         int cir_x,
                         int cir_y,
                         int cir_radius,
                        std::vector<cv::Vec2f>& out_lines) {
  cv::Mat img_cny, img_gry;
  src_gray_image.copyTo(img_gry);

  if (cir_radius != 0) {
     // 求取灰度
    cv::Scalar color_imag_ave = cv::mean(img_gry);
    std::cout << "平均值: " << color_imag_ave.val[0] << " " << color_imag_ave.val[1] << " " << color_imag_ave.val[2] << std::endl;
    double min_value, max_value;    // 最大值，最小值
    cv::Point  min_idx, max_idx;    // 最小值坐标，最大值坐标     
    cv::minMaxLoc(img_gry, &min_value, &max_value, &min_idx, &max_idx);
    std::cout << "最大值: " << max_value <<" 最小值: "<<min_value<<std::endl;
    std::cout << "最大值位置: " << max_idx << " 最小值位置: " << min_idx << std::endl;

    int x_min = cir_x-cir_radius*0.5;
    int x_max = cir_x+cir_radius*0.5;
    int y_min = cir_y-cir_radius*0.5;
    int y_max = cir_y+cir_radius*0.5;
    if (x_min < 0) x_min = 0;
    if (y_min < 0) y_min = 0;
    if (x_max > img_gry.cols) x_max = img_gry.cols;
    if (y_max > img_gry.rows) y_max = img_gry.rows;
    std::cout << "cir_x: " << cir_x << " cir_y: " << cir_y << " cir_radius: " << cir_radius << std::endl;
    std::cout << "cir_center_pixel_value: " << (int)img_gry.at<uint8_t>(cir_y, cir_x) << std::endl;
    // 覆盖圆形所在区域
    for (int row = 0; row < img_gry.rows; row++) {
      for (int col = 0; col < img_gry.cols; col++) {
        if ((row <= y_max) && (row >= y_min) && (col <= x_max) && (col >= x_min)) {
          img_gry.at<uint8_t>(row, col) = color_imag_ave.val[0];
        }
        // if (2 == col) {
        //   std::cout << (int)img_center_fill.at<uint8_t>(row, 2) << std::endl;
        // }
      }
    }
    cv::namedWindow("L center_pixel", cv::WINDOW_FREERATIO);
    cv::imshow("L center_pixel", img_gry);
  }

  // (模糊)均值滤波
  // cv::Mat img_gry_ave_filter;
  // cv::blur(img_gry, img_gry_ave_filter, cv::Size(5, 5));
  // cv::namedWindow("Gray average filter Picture", cv::WINDOW_FREERATIO);
  // cv::imshow("Gray average filter Picture", img_gry_ave_filter);
  // 中值滤波
  cv::Mat img_gry_mid_filter;
  cv::medianBlur(img_gry, img_gry_mid_filter, 3);
#if (SHOW_LINE)
  cv::namedWindow("L mid filter Picture", cv::WINDOW_FREERATIO);
  cv::imshow("L mid filter Picture", img_gry_mid_filter);
#endif
  // 高斯滤波
  cv::Mat img_gaussian_filter;
  cv::GaussianBlur(img_gry_mid_filter, img_gaussian_filter, cv::Size(3, 3), 3, 3);
  cv::GaussianBlur(img_gaussian_filter, img_gaussian_filter, cv::Size(3, 3), 3, 3);
#if (SHOW_LINE)
  cv::namedWindow("L Gaussian filter Picture", cv::WINDOW_FREERATIO);
  cv::imshow("L Gaussian filter Picture", img_gaussian_filter);
#endif
  // 边缘检测
  cv::Canny(img_gry_mid_filter, img_cny, 4, 12, 3, true);
#if (SHOW_LINE)
  cv::namedWindow("L Canny Picture", cv::WINDOW_FREERATIO);
  cv::imshow("L Canny Picture", img_cny);
#endif
   // 标准霍夫变换SHT，直线检测
	cv::HoughLines(img_cny, out_lines, 1, (CV_PI / 180.0), 100);
  
  size_t lines_number = out_lines.size();
	
  return lines_number;
}

bool AlgoriDrtuning::laserSensorSiteAnalysis(cv::Mat& src, cv::Mat& dst) {
  cv::Mat img_rgb, img_gry;

  src.copyTo(img_rgb);
#if (SHOW_AYS)
  cv::namedWindow("Origin Picture", cv::WINDOW_FREERATIO);
  cv::imshow("Origin Picture", img_rgb);
#endif
  cv::cvtColor(img_rgb, img_gry, cv::COLOR_BGR2GRAY);
  cv::Mat img_noise_decade;
  cv::fastNlMeansDenoising(img_gry, img_noise_decade,3, 7, 21); // 去噪
  img_noise_decade.copyTo(img_gry);
#if (SHOW_AYS)
  cv::namedWindow("Gray Picture", cv::WINDOW_FREERATIO);
  cv::imshow("Gray Picture", img_gry);
#endif
  int image_cols = img_gry.cols;
  int image_rows = img_gry.rows;
  cv::Mat img_hough_show;
  img_gry.copyTo(img_hough_show);

  // 找激光中心
  cv::Vec3f cir;
	centerCircleAnalysis(img_gry, cir);
  cv::circle(img_hough_show, cv::Point(cir[0], cir[1]), cir[2], cv::Scalar(255,255,255));
  std::cout << "Cx: " << cir[0] << " Cy: " << cir[1] << " Cr: " << cir[2] << std::endl;
	
  int ccir_x = 0;
  int ccir_y = 0;
  int ccir_radius = 0;  
  ccir_x = cir[0];
  ccir_y = cir[1];
  ccir_radius = cir[2]+10;

  // 找Sensor边缘线
  std::vector<cv::Vec2f> lines;
  struct LinesRelativePosInfo {
    size_t index_line;
    double  distance;
    LinesRelativePosInfo(size_t index_line, double distance) {
      this->index_line = index_line;
      this->distance = distance;
    }
    LinesRelativePosInfo() { }
  };
  std::vector<LinesRelativePosInfo> centroid_up_lines_index;
  std::vector<LinesRelativePosInfo> centroid_down_lines_index;
  centroid_down_lines_index.clear();
  centroid_up_lines_index.clear();

  edgeLinesAnalysis(img_gry, ccir_x, ccir_y, ccir_radius, lines);
	for (size_t i = 0; i < lines.size(); ++i) {
		float rho = lines[i][0];
    float theta = lines[i][1];
		double a = cos(theta);
    double b = sin(theta);
		double x0 = a * rho;
    double y0 = b * rho;
    cv::Point pt1, pt2;
		pt1.x = cvRound(x0 + (-b));
		pt1.y = cvRound(y0 + (a));
		pt2.x = cvRound(x0 - (-b));
		pt2.y = cvRound(y0 - (a));
    if (pt1.x < 0) pt1.x = image_cols + pt1.x;
    if (pt1.y < 0) pt1.y = image_rows + pt1.y;
    if (pt2.x < 0) pt2.x = image_cols + pt2.x;
    if (pt2.y < 0) pt2.y = image_rows + pt2.y;
    std::cout << "lines: " << i << std::endl;
    std::cout << "rho: " << lines[i][0] << " theta(degree): " << (lines[i][1]/CV_PI*180.0) << std::endl;
    std::cout << "x0: " << x0 << " y0: " << y0 << std::endl;
    std::cout << "pt1.x: " << pt1.x << " pt1.y: " << pt1.y << std::endl;
    std::cout << "pt2.x: " << pt2.x << " pt2.y: " << pt2.y << std::endl;
    std::cout << "-------------------" << std::endl;
		cv::line(img_hough_show, pt1, pt2, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
    // 对线与质心的相对位置进行分类，分为圆在线上侧的线和圆在线下侧的线
    if (pt1.x != pt2.x) {
      double k1 = (pt2.y - pt1.y)/(pt2.x - pt1.x);
      double kb1 = pt2.y - k1*pt2.x;
      double centroid_cir_x = cir[0];
      double centroid_cir_y = cir[1];
      // 质心到直线的距离
      double pl_dis = fabs(k1*centroid_cir_x+kb1-centroid_cir_y)/sqrt((k1*k1+1));
      // 将质心坐标代入直线方程，求解两者的相对位置
      double relative_pos = k1*centroid_cir_x+kb1-centroid_cir_y;
      if (relative_pos > 0) {
        std::cout << "line" << i << " up pos, distance: " << pl_dis << std::endl;
        centroid_up_lines_index.push_back(LinesRelativePosInfo(i, pl_dis));
      } else if (relative_pos < 0) {
        std::cout << "line" << i << " down pos, distance: " << pl_dis << std::endl;
        centroid_down_lines_index.push_back(LinesRelativePosInfo(i, pl_dis));
      } else {
        std::cout << "line" << i << " in pos, distance: " << pl_dis << std::endl;
      }
      std::cout << "-------------------" << std::endl;
    }
	}

  std::cout << "centroid up pos line number: " << centroid_up_lines_index.size() << std::endl;
  std::cout << "centroid down pos line number: " << centroid_down_lines_index.size() << std::endl;
  // 求解质心在Sensor上的位置概率
  if ((centroid_down_lines_index.size() != 0) &&
    (centroid_up_lines_index.size() != 0)) {
    std::sort(centroid_down_lines_index.begin(), centroid_down_lines_index.end(), 
      [](LinesRelativePosInfo a, LinesRelativePosInfo b){ return a.distance < b.distance;});
    std::sort(centroid_up_lines_index.begin(), centroid_up_lines_index.end(),
      [](LinesRelativePosInfo a, LinesRelativePosInfo b){ return a.distance < b.distance;});
    double pos_rate = 
      centroid_up_lines_index.front().distance / (centroid_up_lines_index.front().distance + centroid_down_lines_index.front().distance) * 100;
    std::cout << "position rate(%): " << pos_rate << std::endl;
    cv::String pos_rate_str;
    char val[50];
    snprintf(val, 50, "PosRate:%f", pos_rate);
    pos_rate_str = val;
    cv::putText(img_hough_show, 
      pos_rate_str, 
      cv::Point(img_hough_show.cols/2, img_hough_show.rows/2), 
      cv::FONT_HERSHEY_COMPLEX,
      0.5,                      // font face and scale
      cv::Scalar(255, 255, 255),  // white
      1, cv::LINE_AA);
    img_hough_show.copyTo(dst);
#if (SHOW_AYS)
    cv::namedWindow("Trans Picture", cv::WINDOW_FREERATIO);
    cv::imshow("Trans Picture", img_hough_show);
#endif
    return true;
  } else {
    return false;
  }
  
  // std::cout << "------------------------------------" << std::endl;
  // std::cout << "rgb col: " << img_rgb.cols << " rgb row: " << img_rgb.rows << std::endl;
  // std::cout << "rgb channel: " << img_rgb.channels() << std::endl; 
  // std::cout << "gry col: " << img_gry.cols << " gry row: " << img_gry.rows << std::endl;
  // std::cout << "gry channel: " << img_gry.channels() << std::endl; 
  // std::cout << "img_cny mat" << std::endl;
  // for (int row = 0; row < img_cny.rows; row++) {
  //   for (int col = 0; col < 10; col++) {
  //     std::cout << (int)img_cny.at<uint8_t>(row,col) << " ,";
  //   }
  //   std::cout << std::endl;
  // }
}

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/
